package com.zsp.test.djs;

import java.util.*;

public class DijkstraAlgorithm {

    /**
     * 计算两个经纬度坐标之间的球面距离。
     * @param coord1 第一个坐标（纬度, 经度）
     * @param coord2 第二个坐标（纬度, 经度）
     * @return 两个坐标之间的距离（公里）
     */
    public static double haversine(double[] coord1, double[] coord2) {
        final int R = 6371; // 地球半径，单位为公里
        double lat1 = Math.toRadians(coord1[0]);
        double lon1 = Math.toRadians(coord1[1]);
        double lat2 = Math.toRadians(coord2[0]);
        double lon2 = Math.toRadians(coord2[1]);
        double dlat = lat2 - lat1;
        double dlon = lon2 - lon1;
        double a = Math.sin(dlat / 2) * Math.sin(dlat / 2) +
                Math.cos(lat1) * Math.cos(lat2) *
                        Math.sin(dlon / 2) * Math.sin(dlon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        return R * c;
    }

    /**
     * 使用Dijkstra算法寻找从起点到终点的最短路径。
     * @param start 起点坐标
     * @param goal 终点坐标
     * @param graph 图的邻接表表示
     * @return 从起点到终点的最短路径
     */
    public static List<double[]> dijkstra(double[] start, double[] goal, Map<List<Double>, List<double[]>> graph) {
        // 优先队列用于选择当前距离最小的节点
        PriorityQueue<Node> openSet = new PriorityQueue<>(Comparator.comparingDouble(node -> node.distance));
        // 记录路径的来源节点
        Map<double[], double[]> cameFrom = new HashMap<>();
        // 记录从起点到每个节点的最短距离
        Map<double[], Double> distance = new HashMap<>();

        distance.put(start, 0.0); // 起点到起点的距离为0
        openSet.add(new Node(start, 0.0)); // 将起点加入优先队列

        while (!openSet.isEmpty()) {
            Node current = openSet.poll(); // 取出距离最小的节点

            if (Arrays.equals(current.coordinates, goal)) {
                return reconstructPath(cameFrom, current.coordinates); // 找到终点，重建路径
            }

            // 获取当前节点的邻居节点
            List<double[]> neighbors = graph.get(Arrays.asList(current.coordinates[0], current.coordinates[1]));
            if (neighbors != null) {
                for (double[] neighbor : neighbors) {
                    // 计算从起点到邻居节点的距离
                    double tentativeDistance = distance.get(current.coordinates) + haversine(current.coordinates, neighbor);
                    // 如果找到更短的路径，则更新距离和路径
                    if (!distance.containsKey(neighbor) || tentativeDistance < distance.get(neighbor)) {
                        cameFrom.put(neighbor, current.coordinates);
                        distance.put(neighbor, tentativeDistance);
                        openSet.add(new Node(neighbor, tentativeDistance));
                    }
                }
            }
        }
        return null; // 如果没有找到路径，返回null
    }

    /**
     * 从cameFrom字典中重建路径。
     * @param cameFrom 记录路径来源的字典
     * @param current 当前节点
     * @return 完整路径
     */
    private static List<double[]> reconstructPath(Map<double[], double[]> cameFrom, double[] current) {
        List<double[]> totalPath = new ArrayList<>();
        totalPath.add(current);
        while (cameFrom.containsKey(current)) {
            current = cameFrom.get(current);
            totalPath.add(current);
        }
        Collections.reverse(totalPath); // 反转路径，使其从起点到终点
        return totalPath;
    }

    // 节点类，用于存储节点坐标和距离
    static class Node {
        double[] coordinates;
        double distance;

        Node(double[] coordinates, double distance) {
            this.coordinates = coordinates;
            this.distance = distance;
        }
    }

    public static void main(String[] args) {
        // 初始化图的邻接表
        Map<List<Double>, List<double[]>> graph = new HashMap<>();
        graph.put(Arrays.asList(39.9042, 116.4074), Arrays.asList(new double[]{31.2304, 121.4737}, new double[]{34.0522, -118.2437}));
        graph.put(Arrays.asList(31.2304, 121.4737), Arrays.asList(new double[]{39.9042, 116.4074}));
        graph.put(Arrays.asList(34.0522, -118.2437), Arrays.asList(new double[]{39.9042, 116.4074}));

        double[] start = {39.9042, 116.4074}; // 北京
        double[] goal = {31.2304, 121.4737};  // 上海
        List<double[]> path = dijkstra(start, goal, graph);

        if (path != null) {
            System.out.println("Path:");
            for (double[] coord : path) {
                System.out.println(Arrays.toString(coord));
            }
        } else {
            System.out.println("No path found.");
        }
    }
}
